/*
 * This file is part of the Chumby Computer Vision Project: www.EMGRobotics.com
 *
 * Copyright (c) 2010 individual Chumby Computer Vision contributors. 
 * See end of header for contributor list.
 *
 * This code is licensed to you under the terms of the Apache License, version
 * 2.0, or, at your option, the terms of the GNU General Public License,
 * version 2.0. See the APACHE20 and GPL2 files for the text of the licenses,
 * or the following URLs:
 * http://www.apache.org/licenses/LICENSE-2.0
 * http://www.gnu.org/licenses/gpl-2.0.txt
 *
 * If you redistribute this file in source form, modified or unmodified, you
 * may:
 *   1) Leave this header intact and distribute it under the same terms,
 *      accompanying it with the APACHE20 and GPL20 files, or
 *   2) Delete the Apache 2.0 clause and accompany it with the GPL2 file, or
 *   3) Delete the GPL v2 clause and accompany it with the APACHE20 file
 *
 * In all cases you must keep this copyright notice intact, along with the
 * list of contributors at the end of this header.
 *
 * Binary distributions must follow the binary distribution requirements of
 * either License.
 *
 * Contributors: 
 *			Eric Gregori ( www.EMGRobotics.com )
 */
//
//**********************************************************************************
//
//  ChumbyCV Algorithm - Line Following
//
//  Line following is done based on luminance channel only.  Either a bright line
//  on a dark background, or a dark line on a bright background can be tracked. 
//  Breaks image into 9 regions.  At init, assumes center 3 regions contain line.
//  Uses first few frames to determine line threshold.  Line threshold is calculated
//  as the average luminance of the 3 center regions, versus the average luminance
//  of the 6 left and right regions.
//  Threshold is put midway between these averages. 
//
//  If center region average is > then outer region average, assume tracking white line.
//  If outer region average is > then center region average, assume black line.
//
//  outputs: <r1,r2,r3,r4,r5,r6,r7,r8,r9>
//  where:   rx is a number from 0 to 255 representing a ratio of "line" pixels
//	     over background pixesl. 255 = 100% of pixels are "line"
//	     0 = 0% of pixels are "line"
//
//  Command Line Parameters:
//
//
//**********************************************************************************
#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>

#define REGION_WIDTH	106
#define REGION_HEIGHT	60

enum STATE { CALIBRATE, TRACK_WHITE, TRACK_BLACK };

//**********************************************************************************
// CallBack function
//**********************************************************************************
extern inline void SetPixel( uint32_t index, unsigned char r, unsigned char g, unsigned char b );

//**********************************************************************************
// Forward Declarations
//**********************************************************************************
int ChumbyCV_MotionDetect_Init( void );

//**********************************************************************************
// Globals
//**********************************************************************************
uint32_t	G_Width, G_Height;
uint32_t	G_NumOfPixels;
uint32_t	G_FrameCount;			// Good for 4.5 years at 30fps
STATE		G_State;			// 
uint32_t	G_region[4][5];
uint32_t	G_ThresHold;


//**********************************************************************************
// Command Line Parameters:
//	argv[1] - video Device ( /dev/video0 )
//	argv[x] - Options
//			-txxx	Set Threshold ( default operation auto adjusts threshold	
//				Where xxx = decimal # from  1-254
//
// Return 0 for no error, !0 causes exit(1)
//**********************************************************************************
int ChumbyCV_Parameters( int argc, char *argv[], int width, int height )
{
	int	i, bad;

	G_Width  	= width;
	G_Height 	= height;
	G_NumOfPixels	= G_Width*G_Height;
	G_FrameCount	= 0;

	fprintf( stderr, "\nChumbyCV Line Following Algorithm\n" );
	if( argc > 2 )
	{
		for( i=2, bad=1; i<argc; ++i )
		{
			if( argv[i][0] != '-' )
				break;
		
			switch( argv[i][1] )
			{
				case 't':
					G_ThresHold = atoi( &argv[i][2] );
					//G_AutoThres	= 0;
					//if( G_AutoThres < 255 ) bad = 0;
					break;

				case 's':
					//G_Size = atoi( &argv[i][2] );
					bad    = 0;
					break;

				default:
					break;
			}		
		}

		if( bad )
		{
			fprintf( stderr, "\nUsage: %s V4L2_device [-txxx]\n", argv[0] );
			fprintf( stderr, "-txxx Set threshold to decimal xxx ( 0-255 )\n" );
			fprintf( stderr, "default is auto threshold adjust\n" );
			return 1;
		}

	}


	if( ChumbyCV_LineFollower_Init() > 0 )
		return 1;

	return 0; 
}

void ChumbyCV_ShutDown( void )
{
}

//**********************************************************************************
// Init Algorithm
//**********************************************************************************
int ChumbyCV_LineFollower_Init( void )
{
	// There are 4 bytes per 2 pixels in YUVY	
	memset( G_region, 0, 20*sizeof(uint32_t) );
	return 0;
}


//**********************************************************************************
//**********************************************************************************
static inline void DisplayGrid( void )
{
	uint32_t	i,k;

	// vert - 106, 212
	for( i=REGION_WIDTH; i<G_NumOfPixels; i+=G_Width )
 		SetPixel(i,0,255,0);
	for( i=REGION_WIDTH*2; i<G_NumOfPixels; i+=G_Width )
 		SetPixel(i,0,255,0);

	// horz - 60, 120, 180
	k = REGION_HEIGHT*G_Width;
	for( i=k; i<(k+G_Width); ++i )
 		SetPixel(i,0,255,0);
	k = REGION_HEIGHT*2*G_Width;
	for( i=k; i<(k+G_Width); ++i )
 		SetPixel(i,0,255,0);
	k = REGION_HEIGHT*3*G_Width;
	for( i=k; i<(k+G_Width); ++i )
 		SetPixel(i,0,255,0);
}


//**********************************************************************************
// ChumbyCV_Algorithm - Line Follower
//
// This function is called by the ChumbyCV framework.  The char pointer CameraData
// points to a byte array of size (G_NumOfPixels*2).
// The data format is YUYV => 4 bytes = 2 pixels ( Y=pixel, U=color, V=color ) 
//
// Send result to stdio using format: <x,%x>
// Where "x" is the field name, and the value is after the ',' in hex
//**********************************************************************************
int ChumbyCV_Algorithm( unsigned char* CameraData )
{
	uint32_t 	CameraIndex;
	uint32_t 	DisplayIndex;
	unsigned char 	y1, y2, u, v,;

	if( state == CALIBRATE )
	{
		CameraIndex 	= 0;
		for(DisplayIndex=0; DisplayIndex<G_NumOfPixels; ++DisplayIndex )// DisplayIndex -> used by SetPixel
		{
			// YUYV - 2 pixels for 1u, 1v
			y1 	= CameraData[CameraIndex+=2];			// Current frame from camera
			SetPixel(DisplayIndex,y1,y1,y1);			// Display frame
			G_region[(DisplayIndex%G_Width)/REGION_WIDTH][(DisplayIndex/G_Width)/REGION_HEIGHT] += y1;	
		} // for(DisplayIndex=0;

		// Display Grid while calibrating
		DisplayGrid();
		++G_FrameCount;

		// scale region array
		for( u=0; u<3; ++u )
		{
			for( v=0; v<5; ++v )
				G_region[u][v] /= (REGION_WIDTH*REGION_HEIGHT);
		}	

		if( G_FrameCount > 16 )
		{
			// After collecting averages for 16 frames, set threshold, and goto RUN state.
			// Use center region closest to robot as line.
			u = (G_region[0][4]+G_region[2][4])/2;
			if( u > G_region[1][4] )
			{
				// tracking black line
				G_ThresHold = G_region[1][4] + ((u-G_region[1][4])/2);
				state       = TRACK_BLACK;
			}
			else if( u < G_region[1][4] )
			{
				// tracking white line
				G_ThresHold = u + ((G_region[1][4]-u)/2);
				state       = TRACK_WHITE;
			}
			else
			{
				fprintf( stderr, "Error Auto detecting Mode\n" );
				return 1;
			}
		}	
	} // if state
	else
	{
		memset( G_region, 0, 20*sizeof(uint32_t) );
		CameraIndex 	= 0;
		for(DisplayIndex=0; DisplayIndex<G_NumOfPixels; ++DisplayIndex )// DisplayIndex -> used by SetPixel
		{
			// YUYV - 2 pixels for 1u, 1v
			y1 	= CameraData[CameraIndex+=2];			// Current frame from camera
			SetPixel(DisplayIndex,y1,y1,y1);			// Display frame
			G_region[(DisplayIndex%G_Width)/REGION_WIDTH][(DisplayIndex/G_Width)/REGION_HEIGHT] += y1;	
		} // for(DisplayIndex=0;

		// Display Grid while calibrating
		DisplayGrid();
		++G_FrameCount;

#if 0
		CameraIndex 	= 0;
		for(DisplayIndex=0; DisplayIndex<G_NumOfPixels;  )// DisplayIndex -> used by SetPixel
		{
			// YUYV - 2 pixels for 1u, 1v
			y1 	= CameraData[CameraIndex++];			// Current frame from camera
			u  	= CameraData[CameraIndex++];
			y2 	= CameraData[CameraIndex++];
			v  	= CameraData[CameraIndex++];		

			// pixel 1
			SetPixel(DisplayIndex,y1,y1,y1);	
			++DisplayIndex;
			// pixel 2
			SetPixel(DisplayIndex,y2,y2,y2);
			++DisplayIndex;		
		} // for(DisplayIndex=0;
#endif
	} // else state

	// end of Algorithm
	return 0;
}

